/*
 * Copyright (c) 2003 EISLAB, Lulea University of Technology.
 * All rights reserved. 
 * 
 * Redistribution and use in source and binary forms, with or without modification, 
 * are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 *    this list of conditions and the following disclaimer in the documentation
 *    and/or other materials provided with the distribution.
 * 3. The name of the author may not be used to endorse or promote products
 *    derived from this software without specific prior written permission. 
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED 
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
 * SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT 
 * OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING 
 * IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
 * OF SUCH DAMAGE.
 *
 * This file is part of the lwBT Bluetooth stack.
 * 
 * Author: Conny Ohult <conny@sm.luth.se>
 *
 */

/*-----------------------------------------------------------------------------------*/
/* sdp.c
 *
 * Implementation of the service discovery protocol (SDP)
 */
/*-----------------------------------------------------------------------------------*/

#include "netif/lwbt/sdp.h"
#include "netif/lwbt/lwbt_memp.h"
#include "lwbtopts.h"
#include "lwip/debug.h"

#include "lwip/inet.h"

/* Next service record handle to be used */
u32_t rhdl_next;

/* Next transaction id to be used */
u16_t tid_next;

/* The SDP PCB lists */
struct sdp_pcb *sdp_pcbs;
struct sdp_pcb *sdp_tmp_pcb;

/* List of all active service records in the SDP server */
struct sdp_record *sdp_server_records;
struct sdp_record *sdp_tmp_record; /* Only used for temp storage */

/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_init():
 * 
 * Initializes the SDP layer.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_init(void)
{
  /* Clear globals */
  sdp_server_records = NULL;
  sdp_tmp_record = NULL;
  
  /* Inialize service record handles */
  rhdl_next = 0x0000FFFF;
  
  /* Initialize transaction ids */
  tid_next = 0x0000;
}
/*-----------------------------------------------------------------------------------*/
/* Server API
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_next_rhdl():
 * 
 * Issues a service record handler.
 */
/*-----------------------------------------------------------------------------------*/
u32_t
sdp_next_rhdl(void)
{
  ++rhdl_next;
  if(rhdl_next == 0) {
    rhdl_next = 0x0000FFFF;
  }
  return rhdl_next;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_record_new():
 * 
 * Creates a new service record.
 */
/*-----------------------------------------------------------------------------------*/
struct sdp_record *
sdp_record_new(u8_t *record_de_list, u8_t rlen)
{
  struct sdp_record *record;

  record = lwbt_memp_malloc(MEMP_SDP_RECORD);
  if(record != NULL) {
    record->hdl = sdp_next_rhdl();
    record->record_de_list = record_de_list;
    record->len = rlen;
    return record;
  }
  return NULL;
}
/*-----------------------------------------------------------------------------------*/
void
sdp_record_free(struct sdp_record *record)
{
  lwbt_memp_free(MEMP_SDP_RECORD, record);
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_register_service():
 * 
 * Add a record to the list of records in the service record database, making it 
 * available to clients.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_register_service(struct sdp_record *record)
{
  if(record == NULL) {
    return ERR_ARG;
  }
  SDP_RECORD_REG(&sdp_server_records, record);
  return ERR_OK;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_unregister_service():
 * 
 * Remove a record from the list of records in the service record database, making it 
 * unavailable to clients.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_unregister_service(struct sdp_record *record)
{
  SDP_RECORD_RMV(&sdp_server_records, record);
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_next_transid():
 * 
 * Issues a transaction identifier that helps matching a request with the reply.
 */
/*-----------------------------------------------------------------------------------*/
u16_t
sdp_next_transid(void)
{
  ++tid_next;
  return tid_next;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_pattern_search():
 * 
 * Check if the given service search pattern matches the record.
 */
 /*-----------------------------------------------------------------------------------*/
u8_t
sdp_pattern_search(struct sdp_record *record, u8_t size, struct pbuf *p) 
{
  u8_t i, j;
  u8_t *payload = (u8_t *)p->payload;

  for(i = 0; i < size; ++i) {
    if(SDP_DE_TYPE(payload[i]) == SDP_DE_TYPE_UUID)  {
      switch(SDP_DE_SIZE(payload[i])) {
      case SDP_DE_SIZE_16:
	for(j = 0; j < record->len; ++j) {
	  if(SDP_DE_TYPE(record->record_de_list[j]) == SDP_DE_TYPE_UUID) {
	    if(*((u16_t *)(payload + i + 1)) == *((u16_t *)(record->record_de_list + j + 1))) {
	      return 1; /* Found a matching UUID in record */
	    }
	    ++j;
	  }
	}
	i += 2;
	break;
      case SDP_DE_SIZE_32:
	i += 4;
	break;
      case SDP_DE_SIZE_128:
	i+= 16;
	break;
      default:
	break;
      }
    }
  }
  return 0;
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_attribute_search():
 * 
 * Searches a record for attributes and add them to a given packet buffer.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
struct pbuf *
sdp_attribute_search(u16_t max_attribl_bc, struct pbuf *p, struct sdp_record *record) 
{
  struct pbuf *q = NULL;
  struct pbuf *r; 
  struct pbuf *s = NULL;
  u8_t *payload = (u8_t *)p->payload;
  u8_t size;
  u8_t i = 0, j;
  u16_t attr_id = 0, attr_id2 = 0;

  u16_t attribl_bc = 0; /* Byte count of the sevice attributes */
  u32_t hdl = htonl(record->hdl);
  
  if(SDP_DE_TYPE(payload[0]) == SDP_DE_TYPE_DES  && 
     SDP_DE_SIZE(payload[0]) == SDP_DE_SIZE_N1) {
    /* Get size of attribute ID list */
    size = payload[1]; //TODO: correct to assume only one size byte in remote request? probably  
    
    while(i < size) {
      /* Check if this is an attribute ID or a range of attribute IDs */
      if(payload[2+i] == (SDP_DE_TYPE_UINT  | SDP_DE_SIZE_16)) {
	attr_id = *((u16_t *)(payload+3+i));
	attr_id2 = attr_id; /* For the range to cover this attribute ID only */
	i += 3;
      } else if(payload[2+i] == (SDP_DE_TYPE_UINT | SDP_DE_SIZE_32)) {
	attr_id = *((u16_t *)(payload+3+i));
	attr_id2 = *((u16_t *)(payload+5+i));
	i += 5;
      } else {
	/* ERROR: Invalid req syntax */
	//TODO
      }
  
      for(j = 0; j < record->len; ++j) {
	if(SDP_DE_TYPE(record->record_de_list[j]) == SDP_DE_TYPE_DES) {
	  if(record->record_de_list[j + 2] == (SDP_DE_TYPE_UINT | SDP_DE_SIZE_16)) {
	    if(*((u16_t *)(record->record_de_list + j + 3)) >= attr_id && 
	       *((u16_t *)(record->record_de_list + j + 3)) <= attr_id2) {
	      if(attribl_bc +  record->record_de_list[j + 1] + 2 > max_attribl_bc) {
		/* Abort attribute search since attribute list byte count must not 
		   exceed max attribute byte count in req */
		break;
	      }
	      /* Allocate a pbuf for the service attribute */
	      r = pbuf_alloc(PBUF_RAW, record->record_de_list[j + 1], PBUF_RAM);
	      memcpy((u8_t *)r->payload, record->record_de_list + j + 2, r->len);
	      attribl_bc += r->len;

	      /* If request included a service record handle attribute id, add the correct id to the 
		 response */
	      if(*((u16_t *)(record->record_de_list + j + 3)) == 0) {
		memcpy(((u8_t *)r->payload) + 4, &hdl, 4);
	      }

	      /* Add the attribute to the service attribute list */
	      if(s == NULL) {
		s = r;
	      } else {
		pbuf_chain(s, r);
		pbuf_free(r);
	      }
	    }
	  }
	}
      } /* for */
    } /* while */
  } else {
    /* ERROR: Invalid req syntax */
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_attribute_search: Invalid req syntax"));
  }
  /* Return service attribute list */
  if(s != NULL) {
    q = pbuf_alloc(PBUF_RAW, 2, PBUF_RAM);
    ((u8_t *)q->payload)[0] = SDP_DE_TYPE_DES | SDP_DE_SIZE_N1;
    ((u8_t *)q->payload)[1] = s->tot_len;
    pbuf_chain(q, s);
    pbuf_free(s);
  }

  return q;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/*
 * SDP CLIENT API.
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_new():
 * 
 * Creates a new SDP protocol control block but doesn't place it on
 * any of the SDP PCB lists.
 */
/*-----------------------------------------------------------------------------------*/
struct sdp_pcb *
sdp_new(struct l2cap_pcb *l2cappcb)
{
  struct sdp_pcb *pcb;

  pcb = lwbt_memp_malloc(MEMP_SDP_PCB);
  if(pcb != NULL) {
    memset(pcb, 0, sizeof(struct sdp_pcb));
    pcb->l2cappcb = l2cappcb;
    return pcb;
  }
  return NULL;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_free():
 * 
 * Free the SDP protocol control block.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_free(struct sdp_pcb *pcb) 
{
  lwbt_memp_free(MEMP_SDP_PCB, pcb);
  pcb = NULL;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_reset_all():
 * 
 * Free all SDP protocol control blocks and registered records.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_reset_all(void) 
{
  struct sdp_pcb *pcb, *tpcb;
  struct sdp_record *record, *trecord;

  for(pcb = sdp_pcbs; pcb != NULL;) {
    tpcb = pcb->next;
    SDP_RMV(&sdp_pcbs, pcb);
    sdp_free(pcb);
    pcb = tpcb;
  }

  for(record = sdp_server_records; record != NULL;) {
    trecord = record->next;
    sdp_unregister_service(record);
    sdp_record_free(record);
    record = trecord;
  }

  sdp_init();
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_arg():
 *
 * Used to specify the argument that should be passed callback functions.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_arg(struct sdp_pcb *pcb, void *arg)
{
  pcb->callback_arg = arg;
}
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_lp_disconnected():
 *
 * Called by the application to indicate that the lower protocol disconnected.
 */
/*-----------------------------------------------------------------------------------*/
void
sdp_lp_disconnected(struct l2cap_pcb *l2cappcb)
{
  struct sdp_pcb *pcb, *tpcb;

  pcb = sdp_pcbs;
  while(pcb != NULL) {
    tpcb = pcb->next;
    if(bd_addr_cmp(&(l2cappcb->remote_bdaddr), &(pcb->l2cappcb->remote_bdaddr))) {
      /* We do not need to notify upper layer, free PCB */
      sdp_free(pcb);
    }
    pcb = tpcb;
  }
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_req():
 * 
 * Sends a request to a SDP server to locate service records that match the service 
 * search pattern.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_service_search_req(struct sdp_pcb *pcb, u8_t *ssp, u8_t ssplen, u16_t max_src,
		   void (* service_searched)(void *arg, struct sdp_pcb *pcb, u16_t tot_src, 
					     u16_t curr_src, u32_t *rhdls)) 
{
  struct pbuf *p;
  struct sdp_hdr *sdphdr;

  /* Update PCB */
  pcb->tid = sdp_next_transid(); /* Set transaction id */

  /* Allocate packet for PDU hdr + service search pattern + max service record count +
     continuation state */
  p = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+ssplen+2+1, PBUF_RAM);
  sdphdr = p->payload;
  /* Add PDU header to packet */
  sdphdr->pdu = SDP_SS_PDU;
  sdphdr->id = htons(pcb->tid);
  sdphdr->len = htons(ssplen + 3); /* Seq descr + ServiceSearchPattern + MaxServiceRecCount + ContState */

  /* Add service search pattern to packet */
  memcpy(((u8_t *)p->payload) + SDP_PDUHDR_LEN, ssp, ssplen);

  /* Add maximum service record count to packet */
  *((u16_t *)(((u8_t *)p->payload) + ssplen + SDP_PDUHDR_LEN)) = htons(max_src);

  ((u8_t *)p->payload)[SDP_PDUHDR_LEN+ssplen+2] = 0; /* No continuation */
  
  /* Update PCB */
  pcb->service_searched = service_searched; /* Set callback */
  SDP_REG(&sdp_pcbs, pcb); /* Register request */
  
  return l2ca_datawrite(pcb->l2cappcb, p);
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_attrib_req():
 * 
 * Retrieves specified attribute values from a specific service record.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_service_attrib_req(struct sdp_pcb *pcb, u32_t srhdl, u16_t max_abc, u8_t *attrids, u8_t attrlen,
		       void (* attributes_recv)(void *arg, struct sdp_pcb *pcb, u16_t attribl_bc, struct pbuf *p))
{
  struct sdp_hdr *sdphdr;
  u8_t *payload;
  struct pbuf *p;
  
  /* Allocate packet for PDU hdr + service rec hdl + max attribute byte count +
     attribute id data element sequense lenght  + continuation state */
  p = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN + attrlen + 7, PBUF_RAM);

  /* Update PCB */
  pcb->tid = sdp_next_transid(); /* Set transaction id */

  /* Add PDU header to packet */
  sdphdr = p->payload;
  sdphdr->pdu = SDP_SA_PDU;
  sdphdr->id = htons(pcb->tid);
  sdphdr->len = htons((attrlen + 7)); /* Service rec hdl + Max attrib B count + Seq descr + Attribute sequence + ContState */

  payload = p->payload;

  /* Add service record handle to packet */
  *((u32_t *)(payload + SDP_PDUHDR_LEN)) = htonl(srhdl);

  /* Add maximum attribute count to packet */
  *((u16_t *)(payload + SDP_PDUHDR_LEN + 4)) = htons(max_abc);

  /* Add attribute id data element sequence to packet */
  memcpy(payload + SDP_PDUHDR_LEN + 6, attrids, attrlen);
  
  payload[SDP_PDUHDR_LEN + 6 + attrlen] = 0x00; /* No continuation */

  /* Update PCB */
  pcb->attributes_recv = attributes_recv; /* Set callback */
  SDP_REG(&sdp_pcbs, pcb); /* Register request */

  return l2ca_datawrite(pcb->l2cappcb, p);
}
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_attrib_req():
 * 
 * Combines the capabilities of the SDP_ServiceSearchRequest and the 
 * SDP_ServiceAttributeRequest into a single request. Contains both a service search 
 * pattern and a list of attributes to be retrieved from service records that match 
 * the service search pattern.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_service_search_attrib_req(struct sdp_pcb *pcb, u16_t max_abc, u8_t *ssp, u8_t ssplen, u8_t *attrids, 
			      u8_t attrlen, void (* attributes_searched)(void *arg, struct sdp_pcb *pcb, 
									 u16_t attribl_bc, 
									 struct pbuf *p))
{
  struct sdp_hdr *sdphdr;

  struct pbuf *p;
  u8_t *payload;
  u16_t pbuf_bc;

  /* Allocate packet for PDU hdr + service search pattern + max attribute byte count +
     attribute id list + continuation state */
  p = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+ssplen+2+attrlen+1, PBUF_RAM);

  /* Update PCB */
  pcb->tid = sdp_next_transid(); /* Set transaction id */

  /* Add PDU header to packet */
  sdphdr = p->payload;
  sdphdr->pdu = SDP_SSA_PDU;
  sdphdr->id = htons(pcb->tid);
  sdphdr->len = htons(ssplen + 2 + attrlen + 1);
  
  pbuf_bc = SDP_PDUHDR_LEN;
  payload = (u8_t *)p->payload;
  
  /* Add service search pattern to packet */
  memcpy(((u8_t *)p->payload) + SDP_PDUHDR_LEN, ssp, ssplen);
  
  /* Add maximum attribute count to packet */
  *((u16_t *)(payload + SDP_PDUHDR_LEN + ssplen)) = htons(max_abc);

  /* Add attribute id data element sequence to packet */
  memcpy(payload + SDP_PDUHDR_LEN + ssplen + 2, attrids, attrlen);
  
  payload[SDP_PDUHDR_LEN + ssplen + 2 + attrlen] = 0x00; /* No continuation */

  pcb->attributes_searched = attributes_searched; /* Set callback */
  SDP_REG(&sdp_pcbs, pcb); /* Register request */

  return l2ca_datawrite(pcb->l2cappcb, p);
}
/*-----------------------------------------------------------------------------------*/
/*
 * SDP SERVER API.
 */
/*-----------------------------------------------------------------------------------*/
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_rsp():
 * 
 * The SDP server sends a list of service record handles for service records that 
 * match the service search pattern given in the request.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
sdp_service_search_rsp(struct l2cap_pcb *pcb, struct pbuf *p, struct sdp_hdr *reqhdr)
{
  struct sdp_record *record;
  struct sdp_hdr *rsphdr;
  
  struct pbuf *q; /* response packet */
  struct pbuf *r; /* tmp buffer */
  
  u16_t max_src = 0;
  u16_t curr_src = 0;
  u16_t tot_src = 0;

  u8_t size = 0;

  err_t ret;
  
  if(SDP_DE_TYPE(((u8_t *)p->payload)[0]) == SDP_DE_TYPE_DES && 
     SDP_DE_SIZE(((u8_t *)p->payload)[0]) ==  SDP_DE_SIZE_N1) {
    /* Size of the search pattern must be in the next byte since only 
       12 UUIDs are allowed in one pattern */
    size = ((u8_t *)p->payload)[1];
    
    /* Get maximum service record count that follows the service search pattern */
    max_src = ntohs(*((u16_t *)(((u8_t *)p->payload)+(2+size))));

    pbuf_header(p, -2);
  } else {
    //TODO: INVALID SYNTAX ERROR
  }

  /* Allocate header + Total service rec count + Current service rec count  */
  q  = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+4, PBUF_RAM);

  rsphdr = q->payload;
  rsphdr->pdu = SDP_SSR_PDU;
  rsphdr->id = reqhdr->id;

  for(record = sdp_server_records; record != NULL; record = record->next) {
    /* Check if service search pattern matches record */
    if(sdp_pattern_search(record, size, p)) {
      if(max_src > 0) {
	/* Add service record handle to packet */
	r = pbuf_alloc(PBUF_RAW, 4, PBUF_RAM);
	*((u32_t *)r->payload) = htonl(record->hdl);
	pbuf_chain(q, r);
	pbuf_free(r);
	--max_src;
	++curr_src;
      }
      ++tot_src;
    }
  }

  /* Add continuation state to packet */
  r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM);
  ((u8_t *)r->payload)[0] = 0x00;
  pbuf_chain(q, r);
  pbuf_free(r);
 
  /* Add paramenter length to header */
  rsphdr->len = htons(q->tot_len - SDP_PDUHDR_LEN);
  
  /* Add total service record count to packet */
  *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = htons(tot_src);

  /* Add current service record count to packet */
  *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN + 2)) = htons(curr_src);


  {
      u16_t i;
    for(r = q; r != NULL; r = r->next) {
      for(i = 0; i < r->len; ++i) {
	LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_rsp: 0x%x\n", ((u8_t *)r->payload)[i]));
      }
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_rsp: STOP\n"));
    }
    }

  ret = l2ca_datawrite(pcb, q);
  pbuf_free(q);
  return ret;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_attrib_rsp():
 * 
 * Sends a response that contains a list of attributes (both attribute ID and 
 * attribute value) from the requested service record.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
sdp_service_attrib_rsp(struct l2cap_pcb *pcb, struct pbuf *p, struct sdp_hdr *reqhdr)
{
  struct sdp_record *record;
  struct sdp_hdr *rsphdr;

  struct pbuf *q;
  struct pbuf *r;

  u16_t max_attribl_bc = 0; /* Maximum attribute list byte count */

  err_t ret;

  /* Find record */
  for(record = sdp_server_records; record != NULL; record = record->next) {
    if(record->hdl == ntohl(*((u32_t *)p->payload))) {
      break;
    }
  } 
  if(record != NULL) { 
    /* Get maximum attribute byte count */
    max_attribl_bc = ntohs(((u16_t *)p->payload)[2]); 
    
    /* Allocate rsp packet header + Attribute list count */
    q  = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN+2, PBUF_RAM);
    rsphdr = q->payload;
    rsphdr->pdu = SDP_SAR_PDU;
    rsphdr->id = reqhdr->id;

    /* Search for attributes and add them to a pbuf */
    pbuf_header(p, -6);
    r = sdp_attribute_search(max_attribl_bc, p, record);
    if(r != NULL) {
      /* Add attribute list byte count length to header */
      *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = htons(r->tot_len);
      pbuf_chain(q, r); /* Chain attribute id list for service to response packet */
      pbuf_free(r);
    } else {
      *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = 0;
    }
    
    /* Add continuation state to packet */
    r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM);
    ((u8_t *)r->payload)[0] = 0x00; //TODO: Is this correct?
    pbuf_chain(q, r);
    pbuf_free(r);

    /* Add paramenter length to header */
    rsphdr->len = htons(q->tot_len - SDP_PDUHDR_LEN);

    {
      u16_t i;
    for(r = q; r != NULL; r = r->next) {
      for(i = 0; i < r->len; ++i) {
	LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_attrib_rsp: 0x%x\n", ((u8_t *)r->payload)[i]));
      }
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_attrib_rsp: STOP\n"));
    }
    }

    ret = l2ca_datawrite(pcb, q);
    pbuf_free(q);

    return ret;
  }
  //TODO: ERROR NO SERVICE RECORD MATCHING HANDLE FOUND
  return ERR_OK;
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/*
 * sdp_service_search_attrib_rsp():
 * 
 * Sends a response that contains a list of attributes (both attribute ID and 
 * attribute value) from the service records that match the requested service search 
 * pattern.
 */
/*-----------------------------------------------------------------------------------*/
#if LWBT_LAP
err_t
sdp_service_search_attrib_rsp(struct l2cap_pcb *pcb, struct pbuf *p, struct sdp_hdr *reqhdr)
{
  struct sdp_record *record;
  struct sdp_hdr *rsphdr;
  
  struct pbuf *q; /* response packet */
  struct pbuf *r = NULL; /* tmp buffer */
  struct pbuf *s = NULL; /* tmp buffer */

  u16_t max_attribl_bc = 0;
  u8_t size = 0;

  /* Get size of service search pattern */
  if(SDP_DE_TYPE(((u8_t *)p->payload)[0]) == SDP_DE_TYPE_DES && 
     SDP_DE_SIZE(((u8_t *)p->payload)[0]) ==  SDP_DE_SIZE_N1) {
    /* Size of the search pattern must be in the next byte since only 
       12 UUIDs are allowed in one pattern */
    size = ((u8_t *)p->payload)[1];

    /* Get maximum attribute byte count that follows the service search pattern */
    max_attribl_bc = ntohs(*((u16_t *)(((u8_t *)p->payload)+(2+size))));

    pbuf_header(p, -2);
  } else {
    //TODO: INVALID SYNTAX ERROR
  }

  /* Allocate header + attribute list count */
  q  = pbuf_alloc(PBUF_RAW, SDP_PDUHDR_LEN + 2, PBUF_RAM);

  rsphdr = q->payload;
  rsphdr->pdu = SDP_SSAR_PDU;
  rsphdr->id = reqhdr->id;

  for(record = sdp_server_records; record != NULL; record = record->next) {
    /* Check if service search pattern matches record */
    if(sdp_pattern_search(record, size, p)) {
      /* Search for attributes and add them to a pbuf */
      pbuf_header(p, -(size + 2));
      r = sdp_attribute_search(max_attribl_bc, p, record);
      if(r != NULL) {
	if(q->next == NULL) {
	  s = pbuf_alloc(PBUF_RAW, 2, PBUF_RAM);
	  pbuf_chain(q, s); /* Chain attribute id list for service to response packet */
	  pbuf_free(s);
	}
	max_attribl_bc -= r->tot_len; /* Calculate remaining number of bytes of attribute 
						       data the server is to return in response to the 
						       request */
	pbuf_chain(q, r); /* Chain attribute id list for service to response packet */
	pbuf_free(r);
      }
      pbuf_header(p, size + 2);
    }
  }
  
  /* Add attribute list byte count length and length of all attribute lists in this PDU to packet */
  if(q->next != NULL ) {
    *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = htons(q->tot_len - SDP_PDUHDR_LEN - 2);

    ((u8_t *)q->next->payload)[0] = 0x35;
    ((u8_t *)q->next->payload)[1] = q->tot_len - SDP_PDUHDR_LEN - 4;
  } else {
    *((u16_t *)(((u8_t *)q->payload) + SDP_PDUHDR_LEN)) = 0;
  }
  
  /* Add continuation state to packet */
  if((r = pbuf_alloc(PBUF_RAW, 1, PBUF_RAM)) == NULL) {
    //TODO: ERROR
  } else {
    ((u8_t *)r->payload)[0] = 0x00; //TODO: Is this correct?
    pbuf_chain(q, r);
    pbuf_free(r);
  }

  /* Add paramenter length to header */
  rsphdr->len = htons(q->tot_len - SDP_PDUHDR_LEN);

  for(r = q; r != NULL; r = r->next) {
    u8_t i;
    for(i = 0; i < r->len; ++i) {
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_attrib_rsp: 0x%x\n", ((u8_t *)r->payload)[i]));
    }
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_service_search_attrib_rsp: STOP\n"));
  }

  return l2ca_datawrite(pcb, q);
}
#endif /* LWBT_LAP */
/*-----------------------------------------------------------------------------------*/
/* 
 * sdp_recv():
 * 
 * Called by the lower layer. Parses the header and handle the SDP message.
 */
/*-----------------------------------------------------------------------------------*/
err_t
sdp_recv(void *arg, struct l2cap_pcb *pcb, struct pbuf *s, err_t err)
{
  struct sdp_hdr *sdphdr;
  struct sdp_pcb *sdppcb;
  err_t ret = ERR_OK;
  u16_t i;
  struct pbuf *p, *q, *r;

  if(s->len != s->tot_len) {
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Fragmented packet received. Reassemble into one buffer\n"));
    if((p = pbuf_alloc(PBUF_RAW, s->tot_len, PBUF_RAM)) != NULL) {
      i = 0;
      for(r = s; r != NULL; r = r->next) {
	  memcpy(((u8_t *)p->payload) + i, r->payload, r->len);
	  i += r->len;
      }
      pbuf_free(s);
    } else {
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Could not allocate buffer for fragmented packet\n"));
      pbuf_free(s);
      return ERR_MEM; 
    }
  } else {
    p = s;
  }

  for(r = p; r != NULL; r = r->next) {
    for(i = 0; i < r->len; ++i) {
      LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: 0x%x\n", ((u8_t *)r->payload)[i]));
    }
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: STOP\n"));
  }

  sdphdr = p->payload;
  pbuf_header(p, -SDP_PDUHDR_LEN);

  switch(sdphdr->pdu) {
  case SDP_ERR_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Error response 0x%x\n", ntohs(*((u16_t *)p->payload))));
    pbuf_free(p);
    break;
#if LWBT_LAP
  case SDP_SS_PDU: /* Client request */
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search request\n"));
    ret = sdp_service_search_rsp(pcb, p, sdphdr);
    pbuf_free(p);
    break;
#endif /* LWBT_LAP */
  case SDP_SSR_PDU: /* Server response */
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search response\n"));
    /* Find the original request */
    for(sdppcb = sdp_pcbs; sdppcb != NULL; sdppcb = sdppcb->next) {
      if(sdppcb->tid == ntohs(sdphdr->id)) {
	break; /* Found */
      } /* if */
    } /* for */
    if(sdppcb != NULL) {
      /* Unregister the request */
      SDP_RMV(&sdp_pcbs, sdppcb);
      /* Callback function for a service search response */
      SDP_ACTION_SERVICE_SEARCHED(sdppcb, ntohs(((u16_t *)p->payload)[0]), ntohs(((u16_t *)p->payload)[1]), ((u32_t *)p->payload) + 1);
    }
    pbuf_free(p);
    break;
#if LWBT_LAP
  case SDP_SA_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service attribute request\n"));
    ret = sdp_service_attrib_rsp(pcb, p, sdphdr);
    pbuf_free(p);
    break;
#endif /* LWBT_LAP */
  case SDP_SAR_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service attribute response\n"));
    /* Find the original request */
    for(sdppcb = sdp_pcbs; sdppcb != NULL; sdppcb = sdppcb->next) {
      if(sdppcb->tid == ntohs(sdphdr->id)) {
	/* Unregister the request */
	SDP_RMV(&sdp_pcbs, sdppcb);
	/* If packet is divided into several pbufs we need to merge them */
	if(p->next != NULL) {
	  r = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
	  i = 0;
	  for(q = p; q != NULL; q = q->next) {
	    memcpy(((u8_t *)r->payload)+i, q->payload, q->len);
	    i += q->len;
	  }
	  pbuf_free(p);
	  p = r;
	}
	i = *((u16_t *)p->payload);
	pbuf_header(p, -2);	
	/* Callback function for a service attribute response */
	SDP_ACTION_ATTRIB_RECV(sdppcb, i, p);
      } /* if */
    } /* for */
    pbuf_free(p);
    break;
#if LWBT_LAP
  case SDP_SSA_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search attribute request\n"));
    ret = sdp_service_search_attrib_rsp(pcb, p, sdphdr);
    pbuf_free(p);
    break;
#endif /* LWBT_LAP */
  case SDP_SSAR_PDU:
    LWIP_DEBUGF(SDP_DEBUG, ("sdp_recv: Service search attribute response\n"));
    /* Find the original request */
    for(sdppcb = sdp_pcbs; sdppcb != NULL; sdppcb = sdppcb->next) {
      if(sdppcb->tid == ntohs(sdphdr->id)) {
	/* Unregister the request */
	SDP_RMV(&sdp_pcbs, sdppcb);
	/* If packet is divided into several pbufs we need to merge them */
	if(p->next != NULL) {
	  r = pbuf_alloc(PBUF_RAW, p->tot_len, PBUF_RAM);
	  i = 0;
	  for(q = p; q != NULL; q = q->next) {
	    memcpy(((u8_t *)r->payload)+i, q->payload, q->len);
	    i += q->len;
	  }
	  pbuf_free(p);
	  p = r;
	}
	i = *((u16_t *)p->payload);
	pbuf_header(p, -2);
	/* Callback function for a service search attribute response */
	SDP_ACTION_ATTRIB_SEARCHED(sdppcb, i, p);
	break; /* Abort request search */
      } /* if */
    } /* for */
    pbuf_free(p);
    break;
  default:
    //TODO: INVALID SYNTAX ERROR
    break;
  }
  return ret;
}
/*-----------------------------------------------------------------------------------*/
